﻿#include "AutoFocusThread.h"
#include "mainwindow.h"
#include <QDebug>

AutoFocusThread::AutoFocusThread(QObject *parent)
    :QThread(parent),
      m_stop(false),
      m_zSemaphore(0)
{
    m_auto = AUTO::AUTO_IDLE;
    m_win = NULL;
    m_zStep = 0;
    m_countUp = 0;
    m_CountDown = 0;
}

void AutoFocusThread::zMoveFinished()
{
    if(m_zSemaphore.available() == 0)
    {
        m_zSemaphore.release();
    }
}

void AutoFocusThread::setMainWindow(MainWindow *win)
{
    m_win = win;
}

void AutoFocusThread::setCameraImage(Qly::CameraImage *cameraImage)
{
    m_cameraImage = cameraImage;
}

AutoFocusThread::~AutoFocusThread()
{

}
void AutoFocusThread::initSemaphore() // 确保每个 semaphore 都为 0
{
    int z = m_zSemaphore.available();
    if(z)
    {
        m_zSemaphore.acquire(z);
    }
}

bool AutoFocusThread::waitingForZStop()
{
    if( !m_zSemaphore.tryAcquire(1, 3*1000) )
    {
        qWarning() << "zSemaphore timeout, failed!";
        return false;
    }
    return true;
}

void AutoFocusThread::stop()
{
    m_stop = true;
    zMoveFinished();
}

void AutoFocusThread::setROI(QRect ROI)
{
    m_ROI = ROI;
}

void AutoFocusThread::setZPositions(QList<double> zList)
{
    m_zList.clear();
    m_zList = zList;
    m_auto = AUTO::AUTO_LIST;
}

void AutoFocusThread::setZStep(double zStep)
{
    m_zStep = zStep;
    m_auto = AUTO::AUTO_STEP;
}

void AutoFocusThread::run()
{
    m_stop = false;
    initSemaphore();
    switch (m_auto)
    {
    case AUTO::AUTO_LIST:
        autoFocus_list();
        break;
    case AUTO::AUTO_STEP:
        autoFocus_step();
        break;
    default:
        break;
    }
    m_win->setImage(m_bestImage);
    emit autoFocusFinish();
}
//从最高点走到最低点。中间不停。直接计算最佳图像。
void AutoFocusThread::autoFocus_list()
{
    m_cameraImage->startFocus();
    emit zMoveToPos( m_zList[0], 1);
    if( !waitingForZStop() )
    {
        m_cameraImage->stopFocus();
        m_bestImage = m_cameraImage->getBestImage().copy();
        return;
    }
    emit zMoveToPos( m_zList[m_zList.size() - 1], 0.2);
    if( !waitingForZStop() )
    {
        m_cameraImage->stopFocus();
        m_bestImage = m_cameraImage->getBestImage().copy();
        return;
    }
    msleep(50);
    m_cameraImage->stopFocus();
    m_bestImage = m_cameraImage->getBestImage().copy();
    return;
}

// 按照 m_zList 给出的坐标来走，找到最清晰的那个点。
void AutoFocusThread::autoFocus_step()
{
    double best_score = -1;
    double z_best = m_zList[0];
    for(int i = 0; i < m_zList.size(); i++)
    {
        if(m_stop) return;
        double z_mm = m_zList[i];
        emit zMoveToPos(z_mm, 0.5);
        if( !waitingForZStop() )
        {
            return;
        }
        msleep(50);
        QImage image = m_win->getImage();
        QImage iamge2 = image.copy(m_ROI);
        cv::Mat mat = m_focusMeasure.preprocess(iamge2);
        double score = m_focusMeasure.measure(mat);
        qDebug() << "autoFocus z = " << z_mm << ", score = " << score;
        if(score > best_score)
        {
            best_score = score ;
            z_best = z_mm;
            m_bestImage = image.copy();
        }
    }

    qDebug() << "best z = " << z_best;
    emit zMoveToPos(z_best, 0.5);
    waitingForZStop();
}
